
/*
 * hardwareInit.s
 *
 * Created: 12.7.2013 16:03:31
 *  Author: Ferda
 */ 

 .section .text
 .global LowLevelInit
 .global LowLevelUninit
 .global EnableWatchDog
 .global DisableWatchDog
 .global JumpToAddress

 #include <avr/io.h>

 #define WDTCSR_ADDR 0x60
 #define SREG_IO 0x3F
 #define MCUSR_IO 0x34
 #define RAMPZ_IO 0x3B

 /****************************************************
 * Enable watchdog
 *\param R24 prescaller value
 *****************************************************/

EnableWatchDog:
		in		r20, SREG_IO								; store SREG
		cli													; disable interrupts

		wdr													; reset wdr
		ldi		r18, (1 << WDCE) | (1 << WDE)				; enable change 
		ori		r24, (1 << WDE)								; or enable wdt bit
		sts		WDTCSR_ADDR, r18
		sts		WDTCSR_ADDR, r24
				
		out		SREG_IO, r20			; restore SREG
		ret


/****************************************************
 * Disable watchdog
 *****************************************************/

 DisableWatchDog:
		in		r19, SREG_IO					; store SREG
		cli										; disable interrupts	

		wdr
		in		r18, MCUSR_IO					; load MCUSR
		andi	r18, ~(1 << WDRF)				; clear WDRF == > enable clear WDE
		out		MCUSR_IO, r18
			
		ldi		r18, (1 << WDCE) | (1 << WDE)	; enable change and enable set WDCE and WDE
		sts		WDTCSR_ADDR, r18
		sts		WDTCSR_ADDR, r1					; disable wdt

		out		SREG_IO, r19					; restore SREG
		ret

 /****************************************************
 * Jump to specified address
 * 
 *****************************************************/
 
JumpToAddress:
		movw	r30, r24			; low word to Z
		ijmp						; jump


/******************************************************
* Computer CRC16 from program flash 
*
*\param R25:R24	- length in bytes
*\return R25:R24 crc for data
*
*******************************************************/
.global ComputeCRC16

ComputeCRC16:
		lsl		r24				; make length in bytes from lenght in words
		rol		r25
		rol		r26

		movw	r18, r24		; R26:R25:R24 => R20:R19:R18
		movw	r20, r26

		andi	r20, 0x01		; for 1281 only lowest bit is valid
		clr		r24				; R25:R24 is CRC
		clr		r25
		ldi		r23, 0xA0		; R23:R22 is poly
		ldi		r22, 0x01
		clr		ZL				; set start at 0x0000
		clr		ZH
		in		r21, RAMPZ_IO	; store RAMPZ
		push	r21
		clr		r21
		
ComputeCRC16_1:
		out		RAMPZ_IO, r21	; new RAMPZ value
		elpm	r26, Z			; R26 readed byte
		adiw	ZL, 1
		adc		r21, r1
		ldi		r27, 8			; R27 bit counter

		eor		r24, r26

ComputeCRC16_2:
		lsr		r25
		ror		r24
		brcc	ComputeCRC16_3
		eor		r25, r23
		eor		r24, r22

ComputeCRC16_3:
		dec		r27
		brne	ComputeCRC16_2

		clc
		sbci	r18, 1
		sbc		r19, r1
		sbc		r20, r1
		brne	ComputeCRC16_1
		pop		r21
		out		RAMPZ_IO, r21
		ret
/****************************************************/


 .end